//
// Created by 马皓然 on 2025/10/25.
//

#ifndef XIAOMI_TEST_CYBERGEAR_PROTOCOL_H
#define XIAOMI_TEST_CYBERGEAR_PROTOCOL_H
#include <stdint.h>
#include <stdbool.h>
#include "can_cybergear.h" // 包含底层 CAN 驱动的定义

// ------------------------ 协议宏定义 ------------------------

// 1. 通信类型 (Mode) - 位于 CAN ID Bit28~bit24
#define CYBERGEAR_MODE_MOTION_CMD    0x01 // 运动控制命令 (发送角度/速度/力矩/Kp/Kd)
#define CYBERGEAR_MODE_MOTION_FBK    0x02 // 运动控制反馈 (接收角度/速度/力矩)
#define CYBERGEAR_MODE_ENABLE        0x03 // 电机使能运行命令
#define CYBERGEAR_MODE_STOP          0x04 // 电机停止运行命令
#define CYBERGEAR_MODE_READ_PARAM    0x11 // 读取电机参数 (通信类型 17)
#define CYBERGEAR_MODE_WRITE_PARAM   0x12 // 写入电机参数 (通信类型 18)

// 2. 运动控制参数限制 (用于float转uint16/uint8)
// 角度限制: ±2π  rad
#define P_MAX   12.566370614f  // 4 * PI
#define P_MIN  -12.566370614f
// 角速度限制: ±30 rad/s
#define V_MAX   30.0f
#define V_MIN  -30.0f
// 力矩限制: ±10.0 Nm
#define T_MAX   10.0f
#define T_MIN  -10.0f
// Kp/Kd 限制 (需根据手册实际调整)
#define KP_MAX  500.0f
#define KD_MAX  5.0f

// 3. 目标运行模式 (写入参数时使用，Index 0x7005)
#define RUN_MODE_POSITION 0x01 // 位置模式 (默认)
#define RUN_MODE_SPEED    0x02 // 速度模式
#define RUN_MODE_TORQUE   0x03 // 力矩模式



// ------------------------ 协议数据结构 ------------------------

// 运动控制指令数据结构
typedef struct {
    float pos_ref;   // 目标角度 (rad), ±4*PI
    float vel_ref;   // 目标角速度 (rad/s), ±30 rad/s
    float tor_ff;    // 目标力矩前馈 (Nm), ±10.0 Nm
    float kp;        // 比例增益 (Kp), 0~500
    float kd;        // 微分增益 (Kd), 0~5
} CyberGear_MotionCmd_t;
// ------------------------ 函数声明 ------------------------

uint32_t CyberGear_BuildExtID(uint8_t mode, uint8_t motor_id, uint16_t master_id);

uint8_t CyberGear_SendMotionCmd(CAN_HandleTypeDef *hcan, uint8_t motor_id,
                                const CyberGear_MotionCmd_t *cmd);

uint8_t CyberGear_SendModeControl(CAN_HandleTypeDef *hcan, uint8_t motor_id, uint8_t cmd_mode);

uint8_t CyberGear_SendWriteParam(CAN_HandleTypeDef *hcan, uint8_t motor_id,
                                 uint16_t index, uint8_t len, const void *data);

#endif //XIAOMI_TEST_CYBERGEAR_PROTOCOL_H